// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#pragma once

#include <string>

#include <frc/TimedRobot.h>
#include <frc/smartdashboard/SendableChooser.h>
#include <frc/controller/PIDController.h> // PID控制

#include <frc/PS4Controller.h>	//添加PS4的驱动库
#include <frc/XboxController.h> //添加xbox的驱动库

#include <frc/drive/DifferentialDrive.h>
#include <frc/motorcontrol/PWMSparkMax.h> //Spark电机驱动
#include <frc/motorcontrol/VictorSP.h>	  //VictorSP电机驱动
#include <frc/motorcontrol/PWMTalonFX.h>	  //PWMTalonFX电机驱动

#include <frc/Solenoid.h> //电磁阀

// Phoenix的电机库文件
#include <ctre/Phoenix.h>
#include <ctre/phoenix/motorcontrol/can/TalonFX.h>
#include <ctre/phoenix/music/Orchestra.h>
#include "ctre/Phoenix.h"

#include <frc/ADXRS450_Gyro.h> //陀螺仪的库

#include <frc/AddressableLED.h> //LED的库

#include "MiniPID.h" //PID 库

// frc的快捷控制套件
//  #include <frc/drive/DifferentialDrive.h>
//  #include <frc/SpeedControllerGroup.h>//一组电机放一起

class Robot : public frc::TimedRobot
{
public:
	void RobotInit() override;
	void RobotPeriodic() override;
	void AutonomousInit() override;
	void AutonomousPeriodic() override;
	void TeleopInit() override;
	void TeleopPeriodic() override;
	void DisabledInit() override;
	void DisabledPeriodic() override;
	void TestInit() override;
	void TestPeriodic() override;

private:
	frc::SendableChooser<std::string> m_chooser;
	const std::string kAutoNameDefault = "Default";
	const std::string kAutoNameCustom = "My Auto";
	std::string m_autoSelected;

	// 这段是给PS4手柄初始化
	frc::PS4Controller PS4{0}; // PS4 手柄连接在0号USB上

	// 这段是给Xbox手柄初始化
	//  frc::XboxController Xbox{0}; // Xbox 手柄连接在0号USB上

	// 各个键位的初始化
	// 吐槽：学校里这个手柄真的是NTR手柄，怎么PS布局还整了个Xbox的按键
	bool PS4_L3 = 0;		  // PS4 L3 左摇杆 按键 Xbox的LSB
	bool PS4_R3 = 0;		  // PS4 R3 右摇杆 按键 Xbox的RSB
	bool PS4_L1 = 0;		  // PS4 L1键 Xbox LB键
	bool PS4_R1 = 0;		  // PS4 R1键 Xbox RB键
	bool PS4_X = 0;			  // PS4 方块键 Xbox X键
	bool PS4_Y = 0;			  // PS4 三角键 Xbox Y键
	bool PS4_A = 0;			  // PS4 X键 Xbox A键
	bool PS4_B = 0;			  // PS4 O键 Xbox B键
	bool PS4_Share = 0;		  // PS4 Share键 就是左上角的Back
	bool PS4_Options = 0;	  // PS4 Options键 就是右上角的Start
	double PS4_L_Joy_x = 0.0; // PS4 左摇杆 X轴
	double PS4_L_Joy_y = 0.0; // PS4 左摇杆 Y轴
	double PS4_R_Joy_x = 0.0; // PS4 右摇杆 X轴
	double PS4_R_Joy_y = 0.0; // PS4 右摇杆 Y轴
	double PS4_L2 = 0.0;	  // PS4 L2 Xbox LT键
	double PS4_R2 = 0.0;	  // PS4 R2 Xbox RT键

	frc::PWMSparkMax motor_0{0}; // 0 号电机控制器 右前方
	frc::PWMSparkMax motor_1{1}; // 1 号电机控制器 左前方
	frc::PWMSparkMax motor_2{2}; // 2 号电机控制器 右后方
	frc::PWMSparkMax motor_3{3}; // 3 号电机控制器 左后方

	// frc::PWMTalonFX send_ball_0{4};	 // 传球皮带电机， PWM连接
	frc::VictorSP send_ball_1{6};	 // 传球独立轮组电机，PWM连接
	// frc::VictorSP collect_gear{7};	 // 收球轮，PWM连接
	// frc::VictorSP collect_holder{8}; // 收球支架释放，PWM连接，小功率

	double rightSpeed = 0.0; // 右侧速度
	double leftSpeed = 0.0;	 // 左侧速度

	// 定义左右返回值
	// 注意这边因为速度在+-1.0之间浮动所以需要先加1.0来抵消负值
	// 之后再把这个1减掉就好了
	double return_left = 0.0, return_right = 0.0;

	// 请注意CAN线连接的ID
	frc::Solenoid Solenoid_1{frc::PneumaticsModuleType::CTREPCM, 7}; // 1号电磁阀，CAN ID是7
	frc::Solenoid Solenoid_2{frc::PneumaticsModuleType::CTREPCM, 6}; // 2号电磁阀，CAN ID是6

	//https://docs.ctre-phoenix.com/en/stable/ch13_MC.html
	WPI_TalonFX motor_launchBall{2, "rio"}; // 发球电机，CAN 2
	WPI_TalonFX send_ball_0{1, "rio"}; // 传球皮带电机，CAN 1

	frc::ADXRS450_Gyro m_gyro; // 陀螺仪

	static constexpr int kLength = 60;
	frc::AddressableLED m_led{5};								   // 定义一个LED，插在PWM5号口上
	std::array<frc::AddressableLED::LEDData, kLength> m_ledBuffer; // 刷新缓冲器

	int firstPixelHue = 0;

	const double PID_P = 0.7;						// P 误差比例增益，接近目标的速度
	const double PID_I = 0.3;						// I 长时间误差积分增益，最后阶段的接近
	const double PID_D = 0.1;						// D 阻尼微分增益，控制震荡阻尼
	const double PID_F = 1.0;						// F 整体输出比值
	frc2::PIDController m_pid{PID_P, PID_I, PID_D}; // 初始化PID程序

	double SetGyro = 0.0; // 设置陀螺仪目标角度，相对于初始状态

	frc::Timer m_timer; // 计时器

	Orchestra _orchestra;
};